/* ----------------------------------------------------------------------------
 /* Team 3647 
 /* Shooter.java 
 /* Shooter Controler 
 /*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;

public class Shooter {

    private static Victor ShooterCIM     = new Victor(3);
    public static Encoder shooterEncoder = new Encoder(2, 3, true);

    private static double prevCounts = 0, prevTime = 0, rate = 0;
    private static final double a = .5;
    private static final int tol = 10; // Tolerance
    private static int GetRate() {
        double currCounts = shooterEncoder.get();
        double currTime = Timer.getFPGATimestamp();
        double newRate = Math.abs(currCounts - prevCounts) / Math.abs(currTime - prevTime);
        prevCounts = currCounts;
        prevTime = currTime;
        newRate /= 6.0; //covert to RPM
        rate = rate * a + (1 - a) * newRate;
        rate = ((int) (rate / tol)) * tol;
        return (int) rate;
    }
    private static double errorThreshold = 30, fullPower = 1.0, reducedPower = 0.83;

    public static void BangBang(double setpoint) {
        double error = setpoint - GetRate();
        if (error > errorThreshold) {  //If the error is larger than the threshold
            ShooterCIM.set(fullPower);
        } else if (setpoint != 0.0) {  //If the setpoint is nonzero, and the error is within the threshold
            ShooterCIM.set(reducedPower);
        } else {
            ShooterCIM.set(0);
        }
    }
    
    private static double shooterRPMSetPoint = 5900;
    public static void RunShooter() {
        if (JoySticks.ShooterControl2) {
            BangBang(shooterRPMSetPoint);
        } else if (JoySticks.load) {
            ShooterCIM.set(-.56);
        } else {
            BangBang(0);
        }
    }

    public static void ShooterAuton() {
        BangBang(shooterRPMSetPoint);
    }
}
